package Behaviors;

import lejos.robotics.navigation.Pose;
import lejos.robotics.subsumption.Behavior;
import sharedData.MapData;
import sharedData.SharedConstants;
import NXTExplorer.SensorParameter;

/**
 * Forward behavior in subsumption architecture. Does what is says, makes the robot
 * to move forward. In addition to that, it adds the robot position and sensor readings
 * to the data queue.
 * 
 * @author Didzis Petkus, Danny Johansen, Liviu Berheci
 * 
 */
public class Forward implements Behavior {

	private final long SEND_TIME = 250;

	private boolean suppressed = false;

	private long prevTime;

	private SensorParameter paramObj;

	/**
	 * 
	 * @param params
	 *            - the given Java bean object
	 */
	public Forward(SensorParameter params) {

		paramObj = params;

		prevTime = System.currentTimeMillis();
	}

	@Override
	public boolean takeControl() {
		return true;
	}

	@Override
	public void action() {
		suppressed = false;
		paramObj.getPilot().forward();

		while (!suppressed && paramObj.getPilot().isMoving()) {
			if (prevTime + SEND_TIME < System.currentTimeMillis()) {
				handlePosition(paramObj.getPoseProvider().getPose());
				prevTime = System.currentTimeMillis();
			}

			Thread.yield();
		}
	}

	@Override
	public void suppress() {
		suppressed = true;
		paramObj.getPilot().stop();
	}

	/**
	 * Adds the robot position and sensor readings to the data queue based on given pose.
	 * @param pose - The given Pose object
	 */
	private void handlePosition(Pose pose) {
		if (pose == null)
			return;

		MapData data = new MapData(SharedConstants.DATA_TYPE_MAP_DATA,
				pose.getX(), pose.getY(), pose.getHeading(), paramObj
						.getLeftUssr().getRange(), paramObj.getRightUssr()
						.getRange(), paramObj.getCenterUssr().getRange());
		paramObj.getQueue().add(data);
	}
}
